package com.robb19y.rgbdscanner.viewer;

import java.util.ArrayList;

import org.OpenNI.GeneralException;

import com.robb19y.rgbdscanner.core.RGBDData;
import com.robb19y.rgbdscanner.core.RGBDFilter;
import com.robb19y.rgbdscanner.core.RGBDSensor;
import com.robb19y.rgbdscanner.icp.ICPAlgorithm;

public class OffLine {

	/**
	 * @param args
	 */
	public static void main(String[] args) {
		try {
			RGBDSensor camera = new RGBDSensor();
			camera.start();
			Thread.sleep(2000);
			int len = 40;
			ArrayList<RGBDData[][]> listRawData = new ArrayList<RGBDData[][]>(len);
			
			
			Thread.sleep(2000);
			for(int i = 0; i < len; i++){
				Thread.sleep(200);
				//camera.readRGBD();
				listRawData.add(camera.getRgbdData());
				
				System.out.println(i);
			}
			camera.stop();
			
			
			RGBDFilter filter = new RGBDFilter();
			filter.setPixelStep(2);
			ICPAlgorithm icp = new ICPAlgorithm();
			
			RGBDData[][] model = listRawData.get(0);
			
			ArrayList<RGBDData> listModel;
			ArrayList<RGBDData> listData;
			listModel = filter.filter(model);			
			
			RGBDData[][] data;
			
			JVRep vrep = new JVRep();
			vrep.startSimulation();
			for(int i = 1; i < len; i++){
				data = listRawData.get(i);
				
				listData = filter.filter(data);	
				icp.pointCloudRegistration(listModel, listData);
				vrep.addPointCloud(listData);
				
				listModel = listData;
			}
			
			
			
			
			
			
			
			
			
			
			
			
			
			
			
			
		} catch (Exception e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		
		
		
	}

}
